#include "odometry.h"
#include "delay.h"
#include "usart.h"
#include <math.h>

MPU6050 mpu;              // MPU6050 实例
HMC5883L mag;
Wire wire2,wire1;                // I2C 总线
SpeedSensor *ss_left,*ss_right;

OdometryData odom;
float dt;

// 全局保存校准参数
float offset_x=0.138, offset_y=-0.215;
float scale_x=1.050, scale_y=0.954;

void mag_correction(float *mx, float *my) {

    *mx = (*mx - offset_x) * scale_x;
    *my = (*my - offset_y) * scale_y;
}

void odometry_init(float _dt,SpeedSensor *_ss_left,SpeedSensor *_ss_right) { //由上级传入speed_sensor模块
	dt = _dt;
	
	// 初始化 I2C
    Wire_begin(&wire2, I2C2, WIRE_SPEED_DEFAULT);
		while(1) {
		// 初始化 MPU6050
		if (MPU6050_begin(&mpu, &wire2, MPU6050_ADDR) != 0) {
			printf("MPU6050 init failed!\n");
		} else {
			break;
		}
	}

    // 读取 WHO_AM_I 验证
    uint8_t who;
	while(1) {
		if (Wire_readReg(&wire2, MPU6050_ADDR, MPU6050_REG_WHOAMI, &who) == 0) {
			printf("MPU6050 WHO_AM_I = 0x%02X\n", who);
			break;
		} else {
			printf("MPU6050 read failed!\n");
			while(1);
		}
	}

    // 设置量程（推荐：500°/s 陀螺仪）
    MPU6050_setGyroFSScale(&mpu, MPU6050_GYRO_FS_500DPS);   // ±500 °/s
    MPU6050_setAccelFSScale(&mpu, MPU6050_ACCEL_FS_4G);     // ±4g
	
	//
	
	Wire_begin(&wire1, I2C1, WIRE_SPEED_DEFAULT);
	
	if (HMC5883L_Init(&mag, &wire1, HMC5883L_ADDR_DEFAULT) != HMC5883L_OK) {
		printf("HMC5883L not found\n");
        // 错误处理：设备未找到或通信失败
        while(1);
    }
	HMC5883L_SetMode(&mag, HMC5883L_MODE_CONT);
	HMC5883L_Config(&mag, HMC5883L_DEFAULT_SAMPLES,
                           HMC5883L_DEFAULT_RANGE,
                           HMC5883L_RATE_150HZ);
	delay_ms(100);
	uint8_t val;
	HMC5883L_ReadRegs(&mag, HMC5883L_REG_MODE, &val, 1);
	printf("HMC5883L MODE = 0x%02X\n", val);  // 应为 0x00
	HMC5883L_ReadRegs(&mag, HMC5883L_REG_CONFIG_A, &val, 1);
	printf("HMC5883L CONFIG_A = 0x%02X\n", val);
	
	printf("Acc and Gyro calibration in 2s... Keep still!\n");
    delay_ms(2000);

    // ===== 校准陀螺仪和加速度（关键！必须静止）=====
	if (MPU6050_calibrateAccel(&mpu) != 0) {
        printf("acc cali failed\n");
    }

    if (MPU6050_calibrateGyro(&mpu) != 0) {
        printf("calibration failed!\n");
        while (1);
    }
    printf("calibrated.\n");
	
	ss_left = _ss_left;
	ss_right = _ss_right;
	
	// 初始化历史编码器值
    odom.encoder_left  = ss_left->encoder.count;
    odom.encoder_right = ss_right->encoder.count;

    // 初始化位姿
    odometry_reset_to_origin();
    odom.timestamp = micros();
	odom.ax = 0;
	odom.ay = 0;
	odom.az = 0;
	odom.gx = odom.gy = odom.gz = 0;
	odom.mx = odom.my = odom.mz = 0;
	odom.mpu6050_error = 0;
	odom.hmc5883l_error = 0;
}
 
int print_c = 0;
void odometry_update() {
	uint32_t now = micros();
    //float dt = (now - odom.timestamp) / 1000000.0f;  // 真实 dt（秒）
	
	// 1. 读取传感器
	int32_t enc_left = ss_left->encoder.count; //编码器累积值
	int32_t enc_right = ss_right->encoder.count;
	
	// --- MPU6050 ---
	float gx_raw, gy_raw, gz_raw, ax_raw, ay_raw, az_raw;
	int mag_valid;
	if (MPU6050_readRaw(&mpu) == 0) {
		gx_raw = MPU6050_getGx_dps(&mpu) * M_PI / 180.0f;
		gy_raw = MPU6050_getGy_dps(&mpu) * M_PI / 180.0f;
		gz_raw = MPU6050_getGz_dps(&mpu) * M_PI / 180.0f;

		ax_raw = MPU6050_getAx_g(&mpu);
		ay_raw = MPU6050_getAy_g(&mpu);
		az_raw = MPU6050_getAz_g(&mpu);

		// 坐标变换：X_forward → Y_forward（绕 Z 逆时针转 90°）
		odom.gx = gy_raw;
		odom.gy = -gx_raw;
		odom.gz = gz_raw;

		odom.ax = ay_raw;
		odom.ay = -ax_raw;
		odom.az = az_raw;

		// --- HMC5883L ---
		float mx,my,mz;
		if (HMC5883L_ReadScaled(&mag, &mx, &my, &mz) == HMC5883L_OK) {
			mag_valid = !(fabsf(mx) > 1.0f || fabsf(my) > 1.0f || fabsf(mz) > 1.0f);
			if(!mag_valid) {
				printf("MAG Data Error");
			} else {
				mag_correction(&mx,&my);
				odom.mx = mx;
				odom.my = my;
				odom.mz = mz;
			}
			// HMC5883L: Y 向前 → 直接使用
			// mx = 右, my = 前, mz = 下
			// 注意：假设 HMC5883L 安装为 Y 轴朝车头（前），X 轴朝右

			// 计算原始航向角（相对于磁北）
			float mag_from_x = atan2f(odom.my, odom.mx);  // 因为：atan2(Y, X) → 从 X 轴起算
			float mag_theta = -wrap_angle(mag_from_x - M_PI/2.0);
			odom.azimuth = mag_theta;
			//odom.hmc5883l_error = 0;
		} else {
			//printf("MAG ERROR\n");
			odom.mx = odom.my = odom.mz = 0.0f;
			odom.hmc5883l_error = 1;
		}
		//odom.mpu6050_error = 0;
	} else {
		printf("MPU6050 ERROR\n");
		odom.mpu6050_error = 1;
	}

    // 你也可以用 roll/pitch 做倾斜补偿轮速（可选）

	// 2. 计算轮速（m/s）
    float delta_left_m  = (enc_left  - odom.encoder_left)  * TICKS_TO_METER;
    float delta_right_m = - (enc_right - odom.encoder_right) * TICKS_TO_METER;

    float vl = delta_left_m / dt;
    float vr = delta_right_m / dt;

    // ✅ 计算本次走过的弧长（左右轮平均）
    float delta_distance = (delta_left_m + delta_right_m) * 0.5f;
    odom.distance_traveled += delta_distance;

	// 计算线速度和角速度
    float encoder_angular = (vr - vl) / WHEEL_BASE_M;
    odom.linear_vel = (vl + vr) * 0.5f;
	
	//角速度用来更新theta，但使用mahony融合的话这一步就没必要了
    // 可选融合
    //odom.angular_vel = 0.7f * encoder_angular + 0.3f * gz_raw;
	odom.angular_vel = gz_raw;
	// 积分更新位姿
    float dtheta = odom.angular_vel * dt;

	// 计算陀螺预测角（不直接覆盖 odom.theta）
	float gyro_theta = wrap_angle(odom.theta + dtheta);

	// 磁力计相对角（考虑初始偏置）
	float mag_theta = wrap_angle(odom.azimuth - odom.origin_azimuth);

	// 互补滤波融合（若磁力计不可用则退回陀螺预测）
	//const float ALPHA = 0.985f;  // 可调，通常 0.95 ~ 0.995
	const float ALPHA = 1.0f;
	if (mag_valid) {
		odom.theta = wrap_angle(ALPHA * gyro_theta + (1.0f - ALPHA) * mag_theta);
	} else {
		odom.theta = gyro_theta;
	}


    float dy = odom.linear_vel * cosf(odom.theta) * dt;
    float dx = - odom.linear_vel * sinf(odom.theta) * dt;
    odom.x += dx;
    odom.y += dy;

    // 更新历史值
    odom.encoder_left  = enc_left;
    odom.encoder_right = enc_right;
    odom.timestamp = now;
}
/**
 * @brief 重置里程计位姿（可指定初始位置和朝向）
 * @param x 初始 x 坐标（米）
 * @param y 初始 y 坐标（米）
 * @param theta 初始朝向（弧度）
 */
void odometry_reset(float x, float y, float theta) {
    odom.x = x;
    odom.y = y;
    odom.theta = wrap_angle(theta);

    odom.linear_vel = 0.0f;
    odom.angular_vel = 0.0f;

    // 可选：重置编码器历史值（防止下次差分出错）
    // 注意：这不会重置编码器硬件计数，只是里程计内部记录
    if (ss_left && ss_right) {
        odom.encoder_left  = ss_left->encoder.count;
        odom.encoder_right = ss_right->encoder.count;
    }

    // 重置累计行驶距离
    odom.distance_traveled = 0.0f;
	for(int i=0;i<5;i++) {
		float mx,my,mz;
		if (HMC5883L_ReadScaled(&mag, &mx, &my, &mz) == HMC5883L_OK) {
			if(fabsf(mx) > 1.0f || fabsf(my) > 1.0f || fabsf(mz) > 1.0f) {
				printf("MAG Data Error");
			} else {
				mag_correction(&mx,&my);
				odom.mx = mx;
				odom.my = my;
				odom.mz = mz;
			}
			// 计算原始航向角（相对于磁北）
			float mag_from_x = atan2f(odom.my, odom.mx);  // 因为：atan2(Y, X) → 从 X 轴起算
			float mag_theta = -wrap_angle(mag_from_x - M_PI/2.0);
			odom.origin_azimuth = mag_theta;
			odom.azimuth = mag_theta;
			
		} else {
			printf("MAG ERROR\n");
			odom.mx = odom.my = odom.mz = 0.0f;
		}
	}
}

/**
 * @brief 简化版：重置到位姿原点 (0,0,0)
 */
void odometry_reset_to_origin(void) {
    odometry_reset(0.0f, 0.0f, 0.0f);
}

/**
 * @brief 打印当前里程计数据，用于调试
 */
void odometry_print(void)
{
    printf("=== Odometry Data ===\n");
    printf("Timestamp: %u ms\n", odom.timestamp);

    printf("Encoders: L=%ld, R=%ld\n", 
           (long)odom.encoder_left, (long)odom.encoder_right);

    // 打印完整的陀螺仪数据 (角速度)
    printf("Gyro (IMU): X=%.3f, Y=%.3f, Z=%.3f rad/s\n",
           odom.gx, odom.gy, odom.gz);

    // 打印完整的加速度计数据
    printf("Accel (IMU): X=%.3f, Y=%.3f, Z=%.3f m/s²\n",
           odom.ax, odom.ay, odom.az);

    // 打印磁力计数据（如果可用）
    printf("Magnetometer (IMU): X=%.3f, Y=%.3f, Z=%.3f μT\n",
           odom.mx, odom.my, odom.mz);

    // 运动状态
    printf("Velocity: Linear=%.3f m/s, Angular=%.3f rad/s\n",
           odom.linear_vel, odom.angular_vel);

    // 位姿信息
    printf("Pose: X=%.3f m, Y=%.3f m, Theta=%.3f rad (%.1f deg)\n",
           odom.x, odom.y, odom.theta, 
           odom.theta * 180.0f / M_PI);  // 使用 M_PI 更精确

    printf("Distance Traveled: %.3f m\n", odom.distance_traveled);
    printf("=====================\n");
}
/**
 * @brief 将角度归一化到 [-π, π) 范围内
 * @param angle 输入角度（弧度）
 * @return 归一化后的角度（弧度）
 */
float wrap_angle(float angle)
{
    // 使用 fmodf 计算浮点余数
    angle = fmodf(angle + M_PI, 2.0f * M_PI);
    if (angle < 0.0f) {
        angle += 2.0f * M_PI;
    }
    angle -= M_PI;
    return angle;
}
